#ifndef ROBOTSIM_H
#define ROBOTSIM_H

#include "robotmodel.h"
#include <Simulation/ODESurface.h>

class Simulator;
class SimRobotController;

//definitions for internal objects
class WorldSimulation;
class ODETriMesh;
typedef struct dxBody *dBodyID;

class SimRobotController
{
 public:
  SimRobotController();
  ~SimRobotController();
  /// Sets the current feedback control rate
  void setRate(double dt);

  /// Returns the current commanded configuration
  void getCommandedConfig(std::vector<double>& out);
  /// Returns the current commanded velocity
  void getCommandedVelocity(std::vector<double>& out);

  /// Returns the current "sensed" configuration from the simulator
  void getSensedConfig(std::vector<double>& out);
  /// Returns the current "sensed" velocity from the simulator
  void getSensedVelocity(std::vector<double>& out);

  /// Uses a dynamic interpolant to get from the current state to the
  /// desired milestone (with optional ending velocity).  This interpolant
  /// is time-optimal with respect to the velocity and acceleration bounds.
  void setMilestone(const std::vector<double>& q);
  void setMilestone(const std::vector<double>& q,const std::vector<double>& dq);
  /// Sets a rate controller from the current commanded config to move at
  /// rate dq for time dt.
  void setVelocity(const std::vector<double>& dq,double dt);
  /// Same as setMilestone, but appends an interpolant onto an internal
  /// motion queue starting at the current queued end state.
  void addMilestone(const std::vector<double>& q);
  void addMilestone(const std::vector<double>& q,const std::vector<double>& dq);
  /**
   * Upon calling this with set of python functions, the default controller
   * behavior will be overridden.  These functions are
   * - controller(CustomControlFeedback)
   * - savestate() -> string
   * - loadstate(string)
   * Note that if you want to save/load state between problem runs, the
   * custom controller must be set up exactly the same each time.
   * To remove the custom controller, set controller=NULL
   */
  //void setCustom(PyObject* controller,PyObject* savestate=NULL,PyObject* loadstate=NULL);

  int index;
  WorldSimulation* sim;
  /*
  PyObject* customController;
  PyObject* customSaveState;
  PyObject* customLoadState;
  */
};

/*
class CustomControlFeedback
{
 public:
  void getSensorConfig(std::vector<double>& out);
  void getSensorVelocity(std::vector<double>& out);
  void getCommandedTorques(std::vector<double>& out);
  bool getGravity(int link,double out[3]);
  bool getContact(int link,double out[3],double out2[3]);

  void setPIDCommand(const std::vector<double>& qdes,const std::vector<double>& dqdes);
  void setTorqueCommand(const std::vector<double>& t);
  void setVelocityCommand(const std::vector<double>& dq);
  void setPIDJoint(int joint,double qdes,double dqdes);
  void setTorqueJoint(int joint,double t);
  void setVelocityJoint(int joint,double dq);

  int index;
  WorldSimulation* sim;
};
*/

class SimBody
{
 public:
  /// Applies a force and torque about the COM
  void applyWrench(const double f[3],const double t[3]);
  /// Sets the angular velocity and translational velocity 
  void setVelocity(const double w[3],const double v[3]);
  /// Returns the angular velocity and translational velocity 
  void getVelocity(double out[3],double out2[3]);
  void setTransform(const double R[9],double t[3]);
  void getTransform(double out[9],double out2[3]);

  /// Sets the collision padding (useful for thin objects)
  void setCollisionPadding(double padding);
  double getCollisionPadding();

  /// Gets/sets the surface properties
  ODESurfaceProperties* surface();  

  ODETriMesh* mesh;
  dBodyID body;
};

class Simulator
{
 public:
  Simulator(const WorldModel& model);
  ~Simulator();

  /// Resets to the initial state (same as setState(initialState))
  void reset();
  /// Returns the associated world model
  WorldModel getWorld() const;

  /// Returns a Base64 string representing the binary data for the current
  /// simulation state, including controller parameters, etc.
  std::string getState();
  /// Sets the current simulation state from a Base64 string returned by
  /// a prior getState call.
  void setState(const std::string& str);

  /// Advances the simulation by time t, updating the world model from the
  /// simulation state.
  void simulate(double t);

  /// Updates the world model from the current simulation state.  This only
  /// needs to be called if you change the world model and want to revert
  /// back to the simulation state.
  void updateWorld();

  /// Call this to enable contact feedback between the two objects
  /// (arguments are indexes returned by object.getID()).  Contact feedback
  /// has a small overhead so you may want to do this selectively.
  void enableContactFeedback(int obj1,int obj2);
  /// Call this to enable contact feedback between all pairs of objects.
  /// Contact feedback has a small overhead so you may want to do this
  /// selectively.
  void enableContactFeedbackAll();
  /// Returns true if the objects (indexes returned by object.getID()) are in
  /// contact on the current time step
  bool inContact(int aid,int bid);
  /// Returns the list of contacts (x,n,kFriction) at the last time step.
  /// Normals point into object a.
  void getContacts(int aid,int bid,std::vector<std::vector<double> >& out);
  /// Returns the list of contact forces on object a at the last time step
  void getContactForces(int aid,int bid,std::vector<std::vector<double> >& out);
  /// Returns the contact force on object a at the last time step
  void contactForce(int aid,int bid,double out[3]);
  /// Returns true if the objects had contact over the last simulate() call
  bool hadContact(int aid,int bid);
  /// Returns true if the objects had ever separated during the last
  /// simulate() call
  bool hadSeparation(int aid,int bid);
  /// Returns the average contact force on object a over the last simulate()
  /// call
  void meanContactForce(int aid,int bid,double out[3]);

  /// Returns a controller for the indicated robot
  SimRobotController getController(int robot);
  SimRobotController getController(const RobotModel& robot);
  SimBody getBody(const RobotModelLink& link);
  SimBody getBody(const RigidObjectModel& object);
  SimBody getBody(const TerrainModel& terrain);

  /// Sets the overall gravity vector
  void setGravity(const double g[3]);
  /// Sets the internal simulation substep.  Values < 0.01 are recommended.
  void setSimStep(double dt);

  int index;
  WorldModel world;
  WorldSimulation* sim;
  std::string initialState;
};

#endif
